/* iRobot Create Driver */

#include "circle_api.h"
#include "create_driver.h"

extern int sendchar(int c);

u16 swapHighLow(u16 x);

u8 Create_Start( USART_TypeDef* USARTx, u8 mode )
{
    sendchar(CMD_START);
    sendchar(mode);
    return 0xFF;
}


u8 Create_SetBaudRate( USART_TypeDef* USARTx, u8 baud_code)
{
    USART_SendData(USARTx, baud_code);
    return 0xFF;
}


u8 Create_SetMode( USART_TypeDef* USARTx, u8 mode )
{
    switch(mode)
    {
        case MODE_SAFE:
            sendchar(CMD_SAFE);
            break;
        case MODE_FULL:
            sendchar(CMD_FULL);
            break;
        default:
            return 0x00;
            break;
    }
        
    return 0xFF;
}


u8 Create_RunDemo( USART_TypeDef* USARTx, u8 demo )
{
    sendchar(CMD_DEMO);
    sendchar(demo);
    
    return 0xFF;
}


u8 Create_Drive( USART_TypeDef* USARTx, s16 velocity, s16 radius)
{
    sendchar(CMD_DRIVE);
    sendchar((u8)(velocity >> 8));
    sendchar((u8)(velocity & 0x00FF));
    sendchar((u8)(radius >> 8));
    sendchar((u8)(radius & 0x00FF));
    
    return 0xFF;
}


u8 Create_DriveDirect( USART_TypeDef* USARTx, s16 right_velocity, s16 left_velocity)
{
    sendchar(CMD_DRIVE_DIRECT);
    sendchar((u8)(right_velocity >> 8));
    sendchar((u8)(right_velocity & 0x00FF));
    sendchar((u8)(left_velocity >> 8));
    sendchar((u8)(left_velocity & 0x00FF));

    return 0xFF;
}


u8 Create_DriveMillimeters( USART_TypeDef* USARTx, s16 distance, s16 velocity)
{
    if(distance > 0)
        Create_Drive(USARTx, velocity, 0x7FFF);
    else
        Create_Drive(USARTx, -velocity, 0x7FFF);
    sendchar(CMD_WAIT_DISTANCE);
    sendchar((u8)(distance >> 8));
    sendchar((u8)(distance & 0x00FF));
    Create_Drive(USARTx, 0, 0);
    
    return 0xFF;
}


u8 Create_TurnAngle( USART_TypeDef* USARTx, s16 angle, s16 velocity)
{
    if(angle < 0)
        Create_Drive(USARTx, velocity, 0xFFFF);
    else
        Create_Drive(USARTx, velocity, 0x0001);
    sendchar(CMD_WAIT_ANGLE);
    sendchar((u8)(angle >> 8));
    sendchar((u8)(angle & 0x00FF));
    Create_Drive(USARTx, 0, 0);

    return 0xFF;
}


/*******************************************************************************
* Function Name  : sendchar
* Description    : Sends a character using USART1
*               
*
* Input          : c
* Return         : c
*******************************************************************************/
int sendchar (int c) 
{
   while (!(USART1->SR & USART_FLAG_TXE));   
        USART1->DR = (c & 0x1FF);   
    
    return (c);   
}

s16 Create_GetAngle( USART_TypeDef* USARTx )
{
    s16 angle = 0;
    
    while (!(USART1->SR & USART_FLAG_RXNE));

    return ((int)(USART1->DR & 0x1FF));


    sendchar(142);
    sendchar(20);
        
    while(!(USARTx->SR & USART_FLAG_RXNE));
    angle = (int)(USARTx->DR & 0x1FF) << 8;
    while(!(USARTx->SR & USART_FLAG_RXNE));
    angle += (int)(USARTx->DR & 0x1FF);
    
    return angle;
}

s16 Create_GetDistance( USART_TypeDef* USARTx )
{
    static s16 dist = 0;
    
    while(USARTx->SR & USART_FLAG_RXNE != 0)
        dist = USART_ReceiveData(USARTx);

    sendchar(142);
    sendchar(19);
        
    while(USARTx->SR & USART_FLAG_RXNE == 0);
    dist = USART_ReceiveData(USARTx) << 8;
    while(USARTx->SR & USART_FLAG_RXNE == 0);
    dist += USART_ReceiveData(USARTx);
    
    return dist;
}


u8 Create_GetDistAngle( USART_TypeDef* USARTx, s16* dist, s16* angle)
{
    s16 temp = 0;
    
    while(USARTx->SR & USART_FLAG_RXNE)
        temp = (int)(USARTx->DR & 0x1FF);

    sendchar(149);
    sendchar(2);
    sendchar(19);
    sendchar(20);
    
    while(!(USARTx->SR & USART_FLAG_RXNE));
    *dist = (int)(USARTx->DR & 0x1FF) << 8;
    while(!(USARTx->SR & USART_FLAG_RXNE));
    *dist += (int)(USARTx->DR & 0x1FF);
    
    while(!(USARTx->SR & USART_FLAG_RXNE));
    *angle = (int)(USARTx->DR & 0x1FF) << 8;
    while(!(USARTx->SR & USART_FLAG_RXNE));
    *angle += (int)(USARTx->DR & 0x1FF);
    
    return 1;
}

u8 Create_GetBumpers( USART_TypeDef* USARTx )
{
    sendchar(142);
    sendchar(7);
    while(!(USARTx->SR & USART_FLAG_RXNE));
    
    return (u8)(USARTx->DR & 0x1FF);
}

u8 Create_GetData( USART_TypeDef* USARTx, CREATE_DATA* pCD )
{
    u16 temp = 0;
    u32 timer = 0;
    
    if(pCD == 0)
        return 0;
    
    while(USARTx->SR & USART_FLAG_RXNE)
        temp = (u16)(USARTx->DR & 0x1FF);
    
    sendchar(142);
    sendchar(0);

    temp = 0;
    for(temp = 0; temp < 26; temp++)
    {
        timer = getTimeMicro();
        while(!(USARTx->SR & USART_FLAG_RXNE))
        {
            if( getTimeMicro() - timer >= 100000 )
                return 0;
        }
        *((u8*)(pCD) + temp) = (u8)(USARTx->DR & 0x1FF);
    }
    
    pCD->distance = (s16)swapHighLow((u16)pCD->distance);
    pCD->angle = (s16)swapHighLow((u16)pCD->angle);
    pCD->voltage = swapHighLow(pCD->voltage);
    pCD->current = (s16)swapHighLow((u16)pCD->current);
    pCD->battery_charge = (s16)swapHighLow((u16)pCD->battery_charge);
    pCD->battery_capacity = swapHighLow(pCD->battery_capacity);
    
    return 1;
}


u16 swapHighLow(u16 x)
{
    u16 temp;
    
    temp = (x << 8) + (x>>8);
    return temp;
}
